-
Notifications
You must be signed in to change notification settings - Fork 1.4k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Fixed the problem of static layer not restoring old map values for footprint #4824
Fixed the problem of static layer not restoring old map values for footprint #4824
Conversation
@CihatAltiparmak, your PR has failed to build. Please check CI outputs and resolve issues. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I didn't carefully review this yet (Saturday afternoon over coffee!), but wouldn't it be possible for us to simply remove the setConvexPolygonCost
from the internal static layer grid and apply it to the master grid update (master_grid.setConvexPolygonCost())
)?
That would apply it more globally however which might not be wanted in some cases (though I find that generally hard to believe some layers would be set for that and others are not, practically speaking).
Alternatively, you could have setConvexPolygonCost
split into 2 stages. Stage 1 is getting the cells that belong to the footprint
// we assume the polygon is given in the global_frame...
// we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i) {
MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y)) {
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}
std::vector<MapLocation> polygon_cells;
// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);
// NEW: Now return this!
And return those. The second step would be to obtain their values in the loop, store them, and set the new values for FREE. After we do the master grid update, use the originally obtained values to repopulate the static layer. You'd basically just break the setConvexPolygonCost
into 3 methods: (1) get the cells that belong to a convex shape, (2) sets its value for a fixed value (for free), and (3) sets its value to using a vector of input of values stored originally (to reset).
I think that's similar to what you did, but I think this would require alot less changes.
Thank you for give a feedback in a light speed.
Of course, I can. I just wondered your idea about whether adding new method would be useful for future. For instance, I wanna use setConvexCost in such a way that I can update with max value (
I agree with you. I could not get any chance to take a look at other layers, however I can see where this issue comes from. (#4282 ) |
Ah, that is pretty recent. Yeah I think doing the second thing I recommended on splitting up the functions would be the best move here. Then, parameterize if you want to repopulate (see comment in the issue ticket) |
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
ba90d2d
to
9896607
Compare
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Hi @SteveMacenski , If you get a chance, would you take a look at my modifications, I've tried to minimize diffs to ease your work. Btw, I can consider to add the test cases to increase code coverage if you want. Additionaly, I will take a look at the other layers. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Please revert changes to the workflows of the node, I think it creates a number of regressions. I think you should only need to adjust the updateCosts
portion of the implementation. I can see why you did this, wanting to restore
from the previous iteration's stored values, but I think you can do this much more easily by just restoring a given iteration's costs at the end of the updateCosts
method.
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I think what this currently does is clear the footprint on the master grid after update, but that would bypass the set updateWithXYZ()
policies that users can set in order to set how they want updates to be performed (i.e. use max value or have it overwrite). This would lose information like having non-zero costs due to sensor measurements, even if removing the static map values, which wouldn't be a solution without introducing a regression.
This was a good thought / nice clean way to do this while minimizing code changes, but I think we need to do what we discussed previously unless there's another way around that issue. Break up setConvexPolygonCost
into a few methods inside of costmap2d
, use those methods here to obtain the cost values originally in the layer's internal costmap, clear the costmap, update the master grid, then revert the layer's internal costmap.
Maybe: Is there a way to apply the updateWithXYZ()
policies for the following block? That would reduce the number of localized grid operations back to the same we do now (just each one has an extra couple of checks, which should be minimal in terms of computational cost)
if (footprint_clearing_enabled_) {
master_grid.setConvexPolygonCost(transformed_footprint_, nav2_costmap_2d::FREE_SPACE);
}
I also don't understand why updateFootprint
as a method was updated. I think the footprint must be transformed into the current frame before touch
-ing the points to expand the bounds. I also don't think that updateFootprint should be completed if not updating its bounds. Can/should these be reverted?
Firstly, my apologies for long delay due to my exams so on.
I have a question here. why do we have to update the areas with max value or overwrite after clearing the footprint area? Doesn't
I commented on one of your reviews, but I saw this later. You are right. Ignore my comments in that review. I will figure out all of them. |
No need for apologies, life happens 😉
There's a setting The behavior that this PR currently implements is overriding of values. The master costmap is always cleared fully under footprint, rather than having the option to combine using the max value that could already exist in the costmap from another layer or a previous update. Imagine you had a laser scan layer below this one - the measurements from the laser scan would be deleted which represent real data being lost by overriding them. The obstacle layer that processes the laser scan has itself a clear under footprint parameter that would need to be also enabled to deal with that. There could be situations where you might not set clear under footprint for all layers -- so deleting the footprint's costs for the full master grid without consulting its combination policy is important. The clear under footprint is specific to each layer, not to the master costmap itself. The combination policy that the user selected the layer to run at should be respected :-) if the combination policy is 'override', then what you have is exactly the same as doing the footprint clearing on the local internal costmap to the layer. However, if the policy is 'use maximum', then its not the same. Before this PR, the cleared values under the footprint in the static layer would contribute |
…to two methods Signed-off-by: CihatAltiparmak <[email protected]>
Hello, I tried to apply your suggestion. But I have some concerns. I found out that updateFootprint is only executed when clear costmap entirely service is called. This bring about the costmap is updated only when no plan is found. Of course, this depends on how behavior tree is implemented. I need to do some brain storming about it. It is hard to see this issue in I wonder your feedbacks. If I am in the right way, I will continue by adding (From this comment #4824 (review))
I thought a lot about whether updating bounds for footprint was good or bad. I also understand your concern. The unexpected behaviours may occur. Imagine that you don't use inflation layer. mppi may enter the occupied area. So I wanted to do brain-storming here through this channel. Additionally, I used |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Almost perfect, just a couple of architectural changes to make these more portable for other applications, but then good to go! Thanks for your time and effort prototyping this a few ways!
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
Hello, It took some time because I think your suggested names for the methods sounds awkward. In other methods, cell stands only for the locations of cell. So, I decided to use Secondly, I've created the PR about the parameter we added in this PR. You can find it here. Finally, |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Small nitpicks, but otherwise this is perfect and ready!
In what way? Is this significantly slower now? Something you can do is in
That might be good yes. Only set the dynamic param if |
In the latest implementation, the vector consists of the pairs instead of only location. so this will increase the storage size of the vector. When we leave
We cannot handle this in the first loop. It's because the first loop only handles the lines of the polygon. To do that, we need to modify Btw, I will apply your feedbacks today. |
Ok great!
You are right, ignore that 😄 |
- changed the name of the new param - fixed bug in the setConvexPolygonCost - Used reserve method of the vector instances - Added checks in dynamic parameter update. Signed-off-by: CihatAltiparmak <[email protected]>
Hello, I conducted a basic benchmark to measure the effect of our changes. The execution time of my The mean of the default implementation's execution time : ~20000ns You can take a look at how i conducted this benchmark if you want. As you said, I added checks in dynamic parameter update. But I wanna say that setting
I think let's not ignore that. You know the better one, but it can be opened some follow-up issues for that. I feel like the architecture side turned out little bit bad. Anyway. |
Codecov ReportAttention: Patch coverage is
|
I agree, but this is a good thing. The migration guide is the right place for that https://docs.nav2.org/migration/Jazzy.html. You can add in an entry about the new parameter and default value |
Signed-off-by: CihatAltiparmak <[email protected]>
What if you reserve in polygon_map_region there as well? I'm guessing most of those are the vector resizings since std::vector initializes with size of 0 and each time it resizes, it doubles and copies things over. For 100, that would remove ~6-7 copies. |
Seems it speeds up when reserved the vector. But the new rate is ~1.31. I had to wait to get rid of the spikes in the benchmark. (I am also trying the 2^7 :) 🤷♂️ ) The mean of the new implementation's execution time : ~26500ns EDIT: turtlebot occupies 125 cells and I tried 2^7. It is ~25700ns because of allocations. |
What are you measuring, the static layer update time or But, it might be worth keeping that version optimized if need be (if measuring |
Signed-off-by: CihatAltiparmak <[email protected]>
Signed-off-by: CihatAltiparmak <[email protected]>
de7f1b4
to
49916be
Compare
I measured the execution time of the The execution time of Consequently, It increases the computational time by ~8%. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Set the footprint clearing enabled and restore cleared footprint in https://github.com/ros-navigation/navigation2/blob/main/nav2_system_tests/src/system/nav2_system_params.yaml#L196-L198 so that we can have this exercised in a system test, then I can merge!
…tprint enable in nav2_system_tests Signed-off-by: CihatAltiparmak <[email protected]>
…otprint (ros-navigation#4824) * Fix Signed-off-by: CihatAltiparmak <[email protected]> * Fixed some bugs and removed the unnecessary variables from class Signed-off-by: CihatAltiparmak <[email protected]> * Keep the branch updated with main Signed-off-by: CihatAltiparmak <[email protected]> * Called off API changes in costmap, added "restore_outdated_footprint" Signed-off-by: CihatAltiparmak <[email protected]> * Added the documentation for the new methods in static layer Signed-off-by: CihatAltiparmak <[email protected]> * Make CI happy Signed-off-by: CihatAltiparmak <[email protected]> * Apply suggestions and minimize diffs Signed-off-by: CihatAltiparmak <[email protected]> * Fix ament_cpp_lint Signed-off-by: CihatAltiparmak <[email protected]> * Revert changes in the method updateFootprint Signed-off-by: CihatAltiparmak <[email protected]> * Used cached data to restore map and broken up setConvexPolygonCost into two methods Signed-off-by: CihatAltiparmak <[email protected]> * Rename newly added methods Signed-off-by: CihatAltiparmak <[email protected]> * Added restore_outdated_map parameter Signed-off-by: CihatAltiparmak <[email protected]> * Make CI happy Signed-off-by: CihatAltiparmak <[email protected]> * Changed the name of the newly added parameter - changed the name of the new param - fixed bug in the setConvexPolygonCost - Used reserve method of the vector instances - Added checks in dynamic parameter update. Signed-off-by: CihatAltiparmak <[email protected]> * Remove unnecessary log Signed-off-by: CihatAltiparmak <[email protected]> * Add new member to MapLocation Signed-off-by: CihatAltiparmak <[email protected]> * Remove unnecessary header Signed-off-by: CihatAltiparmak <[email protected]> * Set the parameters footprint_clearing_enabled and restore_cleared_footprint enable in nav2_system_tests Signed-off-by: CihatAltiparmak <[email protected]> --------- Signed-off-by: CihatAltiparmak <[email protected]>
Basic Info
Description of contribution in a few bullet points
map_buffer
just after The methodprocessMap
is run so we can utilizemap_buffer
to restore the region cleared before.map_received_in_update_bounds_
because seems we really don't need to.processMap
method because now we are using lock in parts we useprocessMap
Description of documentation updates required from your changes
We have added some methods to Costmap2d. So we may need to update Costmap2D API documentation.
Future work that may be required in bullet points
For Maintainers: